#include "include/gait_phase_detect.hpp"
#include "behaviortree_cpp/bt_factory.h"
#include <behaviortree_cpp/loggers/groot2_publisher.h>
#include <iostream>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include <csignal>
#include <atomic>

std::atomic<bool> shutdown_requested(false);

void signal_handler(int signal) {
    if (signal == SIGINT) {
        shutdown_requested = true;
    }
}
int main()
{
    // 初始化 ROS2 上下文
    if (!rclcpp::ok()) {
        rclcpp::init(0, nullptr);
    }

    // 加载数据
    std::vector<sensorData> csv_data;
    try
    {
        // csv_data = load_csv_data("/home/robot/test/btcpp_sample/exoskeleton1.csv"); // 读取csv文件
        csv_data = load_csv_data("../config/exoskeleton1.csv"); // 读取csv文件
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error loading CSV data: " << e.what() << std::endl;

        return 1;
    }

    if (csv_data.size() < BUFFER_SIZE)
    {
        std::cerr << "Insufficient data in CSV. Need at least " << BUFFER_SIZE << " rows." << std::endl;
        return 1;
    }
    // 加载数据完毕


  BT::BehaviorTreeFactory factory;
  // 插件路径列表
  std::vector<std::string> plugin_paths = {
      "./lib/data_prerocess_action_bt_node.so",
      "./lib/initial_stage_action_bt_node.so",// 逗号
      "./lib/find_heel_strike_bt_node.so",
      "./lib/find_lr_zerocross_bt_node.so",
      "./lib/find_toe_off_bt_node.so"
  };
  
  // 循环加载插件
  std::cout << "Loading behavior tree plugins..." << std::endl;
  for (size_t i = 0; i < plugin_paths.size(); ++i) {
      std::cout << "[" << i+1 << "/" << plugin_paths.size() << "] "
                << "Loading: " << plugin_paths[i] << std::endl;
      
      try {
          factory.registerFromPlugin(plugin_paths[i]);
          std::cout << "✓ Loaded successfully" << std::endl;
      } catch (const std::exception& e) {
          std::cerr << "✗ Failed: " << e.what() << std::endl;
          return EXIT_FAILURE;
      }
  }
  
  std::cout << "All plugins loaded successfully!" << std::endl;


  // factory.registerNodeType<FindToeOff>("FindToeOff");
  // We can add these enums to the scripting language
  factory.registerScriptingEnums<State>();
  //Create Tree
  auto tree = factory.createTreeFromFile("./../config/behavior_trees/bt_tree.xml");
//   auto tree = factory.createTreeFromFile("../bt_tree.xml");

  sensorData data{};// 所有成员默认初始化为 0.0f，默认初始化(C++11 起)
  std::string state;
  auto blackboard = tree.rootBlackboard();
  blackboard->set<bool>("non_firstrun", false); 
  blackboard->set<sensorData>("sensor_data_raw", data); 

  // blackboard->set<std::shared_ptr<std::vector<double>> >("mean_swing_duration", std::make_shared<std::vector<double>>({960}));// 创建空 vector 的共享指针  // blackboard->set("mean_stance_duration", std::make_shared<std::vector<double>>());// 创建空 vector 的共享指针
  // blackboard->set<std::shared_ptr<std::vector<double>> >("mean_stance_duration", std::make_shared<std::vector<double>>({1440}));// 创建空 vector 的共享指针
  // blackboard->set<std::shared_ptr<std::vector<double>> >("mean_T_hs_potential_region", {240});// 创建空 vector 的共享指针
  blackboard->set<std::shared_ptr<std::vector<uint64_t>>>(
      "mean_swing_duration", 
      std::make_shared<std::vector<uint64_t>>(std::initializer_list<uint64_t>{4260}) // 黑板中数组赋初始值
  );
  blackboard->set<std::shared_ptr<std::vector<uint64_t>>>(
      "mean_stance_duration", 
      std::make_shared<std::vector<uint64_t>>(std::initializer_list<uint64_t>{6620})
  );
//   blackboard->set<std::shared_ptr<std::vector<uint64_t>>>(
//       "mean_T_hs_potential_region", 
//       std::make_shared<std::vector<uint64_t>>(std::initializer_list<uint64_t>{840})
//   );
    blackboard->set<std::shared_ptr<std::vector<uint64_t>>>(
        "mean_T_hs_potential_region", 
        nullptr
    );// 初始化为 nullptr

  blackboard->set<std::shared_ptr<std::vector<uint64_t>>>(
      "mean_swing_duration_inlrZC", 
      std::make_shared<std::vector<uint64_t>>(std::initializer_list<uint64_t>{0})
  );
// 从黑板读取
// SensorData data = blackboard->get<sensorData>("sensor_data");


  // 设置信号处理器
  std::signal(SIGINT, signal_handler);

  BT::Groot2Publisher publisher(tree);
  for (uint64_t i = 0; i < csv_data.size() && !shutdown_requested; ++i) //BUFFER_SIZE=1500
  {
      blackboard->set("global_counter", i);
      data = csv_data[i];
      blackboard->set<sensorData>("sensor_data_raw", data);// 每次更新
      data.tof_distance = data.tof_distance - 1;

      
      auto debug_data = blackboard->get<sensorData>("sensor_data_raw");
      // std::cout << "Current data: imu_euler=" << debug_data.imu_euler 
      //           << ", imu_gyr=" << debug_data.imu_gyr 
      //           << ", tof_distance=" << debug_data.tof_distance << std::endl;


    //   std::cout<<"one_tick: "<< i <<std::endl;
      tree.tickExactlyOnce();
      // logData(cnt,data,gait_param);
      std::this_thread::sleep_for(2ms);
  }

  if (shutdown_requested) {
      std::cout << "Shutting down gracefully..." << std::endl;
      publisher.~Groot2Publisher(); // 显式调用析构函数释放资源
  }

  rclcpp::shutdown();

//  while (true)
//  {
//   std::cout<<"one_tick:"<<std::endl;
//   tree.tickExactlyOnce();
//   // tree.tickOnce();
//   // std::this_thread::sleep_for(std::chrono::seconds(1s));
//   std::this_thread::sleep_for(500ms);

//  }



  return 0;
}